# Untitled - By: USER - 周三 8月 2 2023
from machine import UART #串口库函数
from fpioa_manager import fm # GPIO重定向函数
import sensor, image, time,lcd
import time

# uart
fm.register(10, fm.fpioa.UART2_TX, force=True)
fm.register(9, fm.fpioa.UART2_RX, force=True)
uart_A = UART(UART.UART2, 115200, 8, 0, 1, timeout=1000, read_buf_len=4096)
# 阈值
THRESHOLD = (44, 69, -23, 5, -10, 1)# Grayscale threshold for dark things...
white=[(39, 100, -32, 23, -128, 127)]
red_threshold = [(48, 100, 9, 67, -5, 121)]

def color_blob(threshold):

    blobs = img.find_blobs(threshold,x_stride=1, y_stride=1, area_threshold=0,
                           pixels_threshold=0,merge=False,margin=1)

    if len(blobs)>=1 :#有色块
        # Draw a rect around the blob.
        b = blobs[0]
        img.draw_rectangle(b[0:4]) # rect
        cx = b[5]
        cy = b[6]
        for i in range(len(blobs)-1):
            # img.draw_rectangle(b[0:4],color = (0,255,0)) # rect
            cx = blobs[i][5]+cx
            cy = blobs[i][6]+cy
        cx=int(cx/len(blobs))
        cy=int(cy/len(blobs))
        img.draw_cross(cx, cy,color = (255,0,0)) # cx, cy

        return int(cx), int(cy)
    return -1, -1 #表示没有找到

lcd.init(freq=15000000)
lcd.init(type=1)

# 摄像头初始化参数配置
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_windowing((160,120))

sensor.skip_frames(time=2000)     # 跳帧 让摄像头稳定下来
#sensor.set_auto_exposure(True, 80)#在这里调节曝光度，调节完可以比较清晰地看清激光点
#sensor.set_auto_gain(False)#关闭自增益
#sensor.set_brightness(-1) #摄像头亮度，范围为[-2,+2]

sensor.set_hmirror(0)
sensor.set_vflip(0)
clock = time.clock()

# 中心点坐标
center=(0,0)

#模式选择
mode=b'0'

flag=0
x1 = 0
y1 = 0
x2 = 0
y2 = 0
x3 = 0
y3 = 0
x4 = 0
y4 = 0
while(True):

    clock.tick()
    img = sensor.snapshot()
    #数据传输
    data = uart_A.read(1)
    if data==b'0' or data==b'7':
        mode = data
        print(data)

    if b'0'==mode:
        sensor.set_auto_exposure(True, 80)#在这里调节曝光度，调节完可以比较清晰地看清激光点
        sensor.set_auto_gain(False)#关闭自增益
        sensor.set_brightness(-1) #摄像头亮度，范围为[-2,+2]
        cx, cy = color_blob(red_threshold)
        uart_str = "r"+str(cx)+','+str(cy)+"\r\n"
        # print(str(cx),str(cy))
        if cx!=-1:
            uart_A.write("r"+str(cx)+','+str(cy)+"\r\n")
        lcd.display(img, x_scale=1, y_scale=1)
        time.sleep_ms(10)

    if b'7'==mode:
        sensor.set_auto_exposure(False)#在这里调节曝光度，调节完可以比较清晰地看清激光点
        sensor.set_auto_gain(True)#关闭自增益
        sensor.set_brightness(2) #摄像头亮度，范围为[-2,+2]
        img = sensor.snapshot()         # 拍摄图片并返回图像
        img.draw_cross((80,60),color = (255,0,0),size = 5, thickness=1)
        # 在图像中寻找矩形
        for r in  img.find_rects():
            # 判断矩形边长是否符合要求
            if r.w() >60  and r.h() > 40 and r.w() < 80 and r.h() < 50:
                flag += 1
                # 在屏幕上框出矩形
                print("找到了矩形",flag)
                img.draw_rectangle(r.rect(), color = (255, 0, 0), thickness = 2, fill = False)

                # 获取矩形角点位置
                corner = r.corners()

                ## 在屏幕上圆出矩形角点；四个角点坐标, 角点1的数组是corner[0], 坐标就是(corner[0][0],corner[0][1])
                img.draw_circle(corner[0][0], corner[0][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                img.draw_circle(corner[1][0], corner[1][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                img.draw_circle(corner[2][0], corner[2][1], 5, color = (0, 0, 255), thickness = 2, fill = False)
                img.draw_circle(corner[3][0], corner[3][1], 5, color = (0, 255, 0), thickness = 2, fill = False)

                #print("左下角：",corner[0][0], corner[0][1],"，右下角 ",corner[1][0], corner[1][1],"，右上角：",corner[2][0], corner[2][1],"，左上角：",corner[3][0], corner[3][1])
                x1 += corner[0][0]
                y1 += corner[0][1]
                x2 += corner[1][0]
                y2 += corner[1][1]
                x3 += corner[2][0]
                y3 += corner[2][1]
                x4 += corner[3][0]
                y4 += corner[3][1]

        # 在屏幕上显示图像，此部分会降低帧率
        lcd.display(img)
        if(flag %5==0 and x1!=0):
            x1 = x1/5
            y1 = y1/5
            x2 = x2/5
            y2 = y2/5
            x3 = x3/5
            y3 = y3/5
            x4 = x4/5
            y4 = y4/5
            # 将四个角点的坐标数据打包成一个字符串b
            img.draw_rectangle((int(x1),int(y1),int(x2-x1),int(y3-y2)), color = (255, 0, 0))
            corner_str = (str(x1) + ',' + str(y1) + ',' + str(x2) + ',' + str(y2) +
            ',' + str(x3) + ',' + str(y3) + ',' + str(x4) + ',' + str(y4))

            uart_str_a = "a"+str(int(x1))+','+str(int(y1+3))+"\r\n"
            uart_str_b = "b"+str(int(x2))+','+str(int(y2+3))+"\r\n"
            uart_str_c = "c"+str(int(x3))+','+str(int(y3))+"\r\n"
            uart_str_d = "d"+str(int(x4))+','+str(int(y4))+"\r\n"
            print(uart_str_a,uart_str_b,uart_str_c,uart_str_d)
            uart_A.write(uart_str_a)
            time.sleep_ms(20)
            uart_A.write(uart_str_b)
            time.sleep_ms(20)
            uart_A.write(uart_str_c)
            time.sleep_ms(20)
            uart_A.write(uart_str_d)
            time.sleep_ms(20)

            print("@" + corner_str + "!")
            x1 = 0
            y1 = 0
            x2 = 0
            y2 = 0
            x3 = 0
            y3 = 0
            x4 = 0
            y4 = 0
    lcd.display(img, x_scale=1, y_scale=1)
